from Source.HJL import BlockClass
import xml.etree.ElementTree as ET
import numpy as np

# Astar算法获取condition
def getCondition(plist,outdata):
    condition = []
    a = int(outdata[0][3])
    b = int(outdata[1][3])
    for i in range(len(plist)):
        condition.append([a,b])
    # print(condition)
    return condition

# Astar算法获取 goal 和 color
def addArr(plist):
    goal = []
    color = []
    condition = []
    for i in range(len(plist)):
        goal.append([])
        color.append(3)
        condition.append([0,0])
    # print(arrNum)
    return goal,color,condition

# 获取管道尺寸,选择排管方法
def getSize(i,j,pipeBase):
    type = pipeBase._pipeGroups[i].type
    if(type == "Duct" or type == "LineDuct"):
        width = pipeBase._pipeGroups[i]._pipeLists[j]._pipes[0].size.width
        height = pipeBase._pipeGroups[i]._pipeLists[j]._pipes[0].size.height
        # size = width + height
        # if(size >= 1500):
        #     flag = 0
        #     return type,size,flag
        # else:
        #     flag = 0
        #     return type,size,flag
        if (width > 500 or height > 500):
            flag = 0
            return type,width,flag
        else:
            flag = 1
            return type,width,flag
    else:
        DN = pipeBase._pipeGroups[i]._pipeLists[j]._pipes[0].size.width
        if(DN >= 150):
            flag = 0
            return type,DN,flag
        else:
            flag = 1
            return type,DN,flag


# 添加障碍物对象
def add(listArr,blockBase):
    for oneArr in listArr:
        nowBlock = BlockClass.Block()
        nowBlock.type = "Pipe"
        nowBlock.start = oneArr[0]
        nowBlock.end = oneArr[1]
        nowBlock.size.width = oneArr[2]
        nowBlock.size.height = oneArr[3]
        blockBase._blocks.append(nowBlock)

# 获取管道组 pipeGroup
def pipes_getGroup(pipeBase):
    baseArr=[];
    for pipeGroup in pipeBase._pipeGroups:
        groupArr = [];
        for pipeList in pipeGroup._pipeLists:
            listArr=[];
            for pipe in pipeList._pipes:
                # todo 2023-12-7cmf修改，添加if_successful_avoid
                pipeArr=[pipe.start,pipe.end,pipe.size.width,pipe.size.height,pipe.angle,pipe.if_successful_avoid];
                listArr.append(pipeArr);
            groupArr.append(listArr);
        baseArr.append(groupArr);
    return baseArr

# 对不同管道进行分类，所有管道配合sizeOrder按顺序需要分为： 大风管 -> 大水管 -> 小风管 -> 小水管
# def pipeSort(pipeBase):
#     bigDuctList = []
#     bigPipeList = []
#     smallDuctList = []
#     smallPipeList = []
#     pipeGroups = pipes_getGroup(pipeBase)
#     for i in range(len(pipeGroups)):
#         type = pipeBase._pipeGroups[i].type
#         group = pipeGroups[i]
#         for j in range(len(group)):
#             list = pipeGroups[i]
#             for k in range(len(list)):
#                 width = pipeBase._pipeGroups[i]._pipeLists[j]._pipes[k].size.width
#                 height = pipeBase._pipeGroups[i]._pipeLists[j]._pipes[k].size.height
#                 if (type == "Duct" or type == "LineDuct"):
#                     # 宽+高大于2000当大风管
#                     if (width + height > 2000):
#                         bigDuctList.append(pipeBase._pipeGroups[i]._pipeLists[j]._pipes[k])
#                     else:
#                         smallDuctList.append(pipeBase._pipeGroups[i]._pipeLists[j]._pipes[k])
#                 else:
#                     # DN大于200当大水管
#                     if (width + height > 400):
#                         bigPipeList.append(pipeBase._pipeGroups[i]._pipeLists[j]._pipes[k])
#                     else:
#                         smallPipeList.append(pipeBase._pipeGroups[i]._pipeLists[j]._pipes[k])
#     return bigDuctList,bigPipeList,smallDuctList,smallPipeList


# def sizeOrder(pipeBase):
#     newSortList =[]
#     pipesort = pipeSort(pipeBase)
#     # pipesort是一个4种管道类型的数组，需要对每个类型的数组进行sizeOrder
#     # [[],[],[],[]]
#     for i in range(len(pipesort)):
#         single = pipeSort[i]
#         # 此处需要先看一下pipesort的数据结构是怎么表示的，在处理数据！！！
#         width = single[3]
#         height = single[4]
#         single.sort(
#             key=lambda elem: width + height,
#             reverse=True)
#         newSortList.append(single)
#     # pipeBase._pipeGroups.sort(
#     #     key=lambda elem: elem._pipeLists[0]._pipes[0].size.width + elem._pipeLists[0]._pipes[0].size.height,
#     #     reverse=True)

def Compare(StartHeight, EndHeight):
    D_value = abs(StartHeight - EndHeight)
    if D_value < 100:
        return StartHeight
    else:
        flag1 = (StartHeight < 6000) and (StartHeight > 2000)
        flag2 = (EndHeight < 6000) and (EndHeight > 2000)
        if flag1 and flag2:
            return StartHeight if (StartHeight - EndHeight) < 0 else EndHeight
        elif flag1==1 and flag2==0:
            return StartHeight
        elif flag1==0 and flag2==1:
            return EndHeight
        elif flag1==0 and flag2==0:
            return

def GetHeight(dataXml):
    tree = ET.parse(dataXml)
    root = tree.getroot()
    Entitys =root.find('Entitys')
    FrameDatas = []
    for Entity in Entitys:
        if Entity.tag=="Entity":
            Type = Entity.attrib['type']
            if (Type == 'Frame'):
                LocationEnt = Entity.find('LocationEnt')
                StartPoint = LocationEnt.attrib['StartPoint']
                StartHeight = StartPoint.split(',')[2]
                StartHeight = float(StartHeight)
                EndPoint = LocationEnt.attrib['EndPoint']
                EndHeight = EndPoint.split(',')[2]
                EndHeight = float(EndHeight)
                FrameData = Compare(StartHeight, EndHeight)
                if FrameData != None:
                    FrameDatas.append(FrameData)
    if FrameDatas != []:
        return FrameDatas
    else:
        print('frame信息不存在')
        return


def ChangePipelineH(Groups, new_height):
    for Group in Groups:
        for Column in Group:
            for Part in Column:
                Part[0][2] = new_height
                Part[1][2] = new_height
    return Groups

#需在主函数指定以下两个路径并传入setHeight函数中，该函数还需传入要修改的多维数组
# configXml_path = "configXml路径"
# dataXml_path =  = "dataXml路径"
def setHeight(dataXml,configXml,pipegroup):
    new_height = 0 #数组需要修改的新的Z坐标值，也就是以下判断语句提取的
    F9_floor = 53400
    tree = ET.parse(configXml)
    root = tree.getroot()
    wall_height = root.find('OtherSettings').find('GlobalFalseCeiling').find('height').text
    new_heights = GetHeight(dataXml)  # 获取frame的z坐标
    if(wall_height != None):
        new_height = float(wall_height) + F9_floor -100
        #new_height = float(wall_height) + 13000 - 100
    elif new_heights==None:
        return pipegroup
    else:
        new_height = new_heights[0] #若new_heights不为空，则默认取第一个值
    return ChangePipelineH(pipegroup, new_height) #修改管道坐标


#输入起点终点宽高，输出8个点,构成结构的立体图
def dian2(p1, p2, width, height):  # 第二种即两个中心点z不一样
    u = 0
    p3 = np.zeros(3)
    p4 = np.zeros(3)
    p5 = np.zeros(3)
    p6 = np.zeros(3)
    p_1 = np.zeros(3)
    p_2 = np.zeros(3)
    p_3 = np.zeros(3)
    p_4 = np.zeros(3)
    p_5 = np.zeros(3)
    p_6 = np.zeros(3)
    p_7 = np.zeros(3)
    p_8 = np.zeros(3)
    if p1[1] - p2[1] == 0:
        u = 1
    if u == 0:
        k = -(p1[0] - p2[0]) / (p1[1] - p2[1])
        x = width / (2 * (1 + k ** 2) ** 0.5)
        p3[0] = p1[0] + x
        p3[1] = p1[1] + k * x
        p3[2] = p1[2]
        p4[0] = p1[0] - x
        p4[1] = p1[1] - k * x
        p4[2] = p1[2]
        p5[0] = p2[0] + x
        p5[1] = p2[1] + k * x
        p5[2] = p2[2]
        p6[0] = p2[0] - x
        p6[1] = p2[1] - k * x
        p6[2] = p2[2]
    else:
        p3[0] = p1[0]
        p3[1] = p1[1] + width / 2
        p3[2] = p1[2]
        p4[0] = p1[0]
        p4[1] = p1[1] - width / 2
        p4[2] = p1[2]
        p5[0] = p2[0]
        p5[1] = p2[1] + width / 2
        p5[2] = p2[2]
        p6[0] = p2[0]
        p6[1] = p2[1] - width / 2
        p6[2] = p2[2]
    a = (p2[1] - p1[1]) * (p3[2] - p1[2]) - (p3[1] - p1[1]) * (p2[2] - p1[2])
    b = (p2[2] - p1[2]) * (p3[0] - p1[0]) - (p3[2] - p1[2]) * (p2[0] - p1[0])
    c = (p2[0] - p1[0]) * (p3[1] - p1[1]) - (p3[0] - p1[0]) * (p2[1] - p1[1])
    t = height / (2 * (a ** 2 + b ** 2 + c ** 2) ** 0.5)
    p_3[0] = p3[0] + a * t
    p_3[1] = p3[1] + b * t
    p_3[2] = p3[2] + c * t
    p_2[0] = p3[0] - a * t
    p_2[1] = p3[1] - b * t
    p_2[2] = p3[2] - c * t
    p_4[0] = p4[0] + a * t
    p_4[1] = p4[1] + b * t
    p_4[2] = p4[2] + c * t
    p_1[0] = p4[0] - a * t
    p_1[1] = p4[1] - b * t
    p_1[2] = p4[2] - c * t
    p_7[0] = p5[0] + a * t
    p_7[1] = p5[1] + b * t
    p_7[2] = p5[2] + c * t
    p_6[0] = p5[0] - a * t
    p_6[1] = p5[1] - b * t
    p_6[2] = p5[2] - c * t
    p_8[0] = p6[0] + a * t
    p_8[1] = p6[1] + b * t
    p_8[2] = p6[2] + c * t
    p_5[0] = p6[0] - a * t
    p_5[1] = p6[1] - b * t
    p_5[2] = p6[2] - c * t
    list = [p_3, p_7, p_6, p_2, p_4, p_8, p_5, p_1]
    new = []
    for i in list:
        a = np.ndarray.tolist(i)
        new.append(a)
    # print(new)
    return new

#输入为point:[x,y,z];cuboid_xyz:[[x,y,z],[x,y,z]...],若point在内部，则返回True
def point_inner_check(point,cuboid_xyz):
    x0,x1,y0,y1,z0,z1 = cuboid_xyz[0][0],cuboid_xyz[0][0],cuboid_xyz[0][1],cuboid_xyz[0][1],cuboid_xyz[0][2],cuboid_xyz[0][2]
    #找到八个坐标中的最小和最大值:x0,x1,y0,y1,z0,z1
    for i in cuboid_xyz:
        if i[0] < x0:
            x0 = i[0]
        if i[0] > x1:
            x1 = i[0]
        if i[1] < y0:
            y0 = i[1]
        if i[1] > y1:
            y1 = i[1]
        if i[2] < z0:
            z0 = i[2]
        if i[2] > z1:
            z1 = i[2]
    x,y,z = point[0],point[1],point[2]
    # print(x,x0,x1,y,y0,y1,z,z0,z1)
    if(x > x0 and x < x1 and y > y0 and y<y1 and z > z0 and z < z1):
        return True
    else:
        return False

#边沿检测函数，输入边缘距离expansion_distance，管道起点point，以及障碍物坐标block:[[x,y,z],[x,y,z],w,h],如果管道距离边缘在此范围之内，则返回True
def edge_detection(expansion_distance,point,block):
    cuboid_xyz = dian2(block[0], block[1], block[2]+expansion_distance, block[3]+expansion_distance)
    if point_inner_check(point, cuboid_xyz):
        return True
    else:
        return False

#移动的方向和距离函数，输入方向和距离，返回移动后的pipe_start
def move_direction_and_distance(direction,distance,pipe_start):
    if direction == 'x+':
        pipe_start[0] += distance
    elif direction == 'x-':
        pipe_start[0] -= distance
    elif direction == 'y+':
        pipe_start[1] -= distance
    elif direction == 'y-':
        pipe_start[1] -= distance
    elif direction == 'z+':
        pipe_start[2] += distance
    elif direction == 'z-':
        pipe_start[2] -= distance
    # return pipe_start

#输入管道和障碍物列表，检测当前管道起点或终点是否在障碍物内部，或者是否离边缘很近，如果是，则返回当前障碍物坐标。
def block_selection(pipe,block_list):
    for block in block_list:
        cuboid_xyz = dian2(block[0], block[1], block[2], block[3])
        expansion_distance = 0
        if (point_inner_check(pipe[0], cuboid_xyz) or point_inner_check(pipe[1], cuboid_xyz)) or edge_detection(expansion_distance, pipe[0], block):
            return block
            break
        else:
            return False

#输入管道和附近障碍物数组,若管道的起点或终点在障碍物里，则进行下移修正，且修正上限为1000
def pipes_move(pipeGroups,block_list):
    for pipeGroup in pipeGroups:
        for pipeList in pipeGroup:
            for pipe in pipeList:
                # for block in block_list:
                block = block_selection(pipe, block_list)
                if (block != False and type(block) != 'NoneType'):
                # if( block!= False):
                    cuboid_xyz = dian2(block[0], block[1], block[2], block[3])
                    count1 = 0
                    #若管道的起点或终点在障碍物里，则进行下移修正，且修正上限为1000
                    while ((point_inner_check(pipe[0], cuboid_xyz) or point_inner_check(pipe[1], cuboid_xyz)) and (count1 < 1000)):
                        move_direction_and_distance('z-', 20, pipe[0])
                        move_direction_and_distance('z-', 20, pipe[1])
                        count1 += 20
                    #若管道起点距离障碍物边缘在expansion_distance距离之内,则进行下移修正，且修正上限为1000
                    expansion_distance = 100#边缘距离
                    count2 = 0
                    # if edge_detection(expansion_distance, pipe[0], block):
                    while edge_detection(expansion_distance, pipe[0], block) and count2 < 1000:
                            move_direction_and_distance('z-', 20, pipe[0])
                            move_direction_and_distance('z-', 20, pipe[1])
                            count2 += 20
                    move_direction_and_distance('z-',pipe[3]/2, pipe[0])
                    move_direction_and_distance('z-',pipe[3]/2, pipe[1])
                    count1+=pipe[3]/2
                    count2+=pipe[3]/2
    # print('由于管道的起点或终点在障碍物里,进行的修正距离为:', count1)
    # print('由于管道起点距离障碍物边缘太近,无法排管,进行的修正距离为:', count2)
    return pipeGroups
#设置管道的初始高度，9.21新增，直接改变类pipebase
def set_Initial_height(pipeBase, ceilling):
    real_ceilling = ceilling[1]
    max_height = 0
    pipe_height = 0
    for PipeGroup in pipeBase._pipeGroups:
        for pipeList in PipeGroup._pipeLists:
            for pipe in pipeList._pipes:
                if max_height < pipe.start[2]:
                    max_height = pipe.start[2]
                    pipe_height = pipe.size.height
    rectify = real_ceilling - 100 - pipe_height/2 - max_height
    for PipeGroup in pipeBase._pipeGroups:
        for pipeList in PipeGroup._pipeLists:
            for pipe in pipeList._pipes:
                pipe.start[2] += rectify
                pipe.end[2] +=rectify